import ctypes
import logging
import os
import sys
import time

logging.basicConfig(filename='tool.log', level=logging.DEBUG, format='%(asctime)s %(levelname)s: %(message)s')


#   调用结构体 POSE
class POSE(ctypes.Structure):
    _fields_ = [("px", ctypes.c_float),
                ("py", ctypes.c_float),
                ("pz", ctypes.c_float),
                ("rx", ctypes.c_float),
                ("ry", ctypes.c_float),
                ("rz", ctypes.c_float)]


class FRAME(ctypes.Structure):
    _fields_ = [("frame_name", ctypes.c_char * 10),
                ("pose", POSE),
                ("payload", ctypes.c_float),
                ("x", ctypes.c_float),
                ("y", ctypes.c_float),
                ("z", ctypes.c_float)]


class Arm:
    def __init__(self, ip=''):
        self.ip = ip
        CUR_PATH = os.path.dirname(os.path.realpath(__file__))
        dllPath = os.path.join(CUR_PATH, "RM_Base.dll")
        self.pDll = ctypes.cdll.LoadLibrary(dllPath)
        #   API 初始化
        self.pDll.RM_API_Init(65, 0)

        #   连接机械臂
        byteIP = bytes(self.ip, "gbk")
        self.nSocket = self.pDll.Arm_Socket_Start(byteIP, 8080, 2000)
        logging.info(f'连接机械臂完毕')
        self.ret = self.pDll.Arm_Socket_State(self.nSocket)
        logging.info(f'连接状态：{self.ret}')

    def tool_change(self):
        """
        快换工具激光笔
        :return:
        """
        #   运动至激光笔工具上方
        float_joint = ctypes.c_float * 6
        joint = float_joint(*[131.682, -4.102, -63.948, -0.335, -110.513, -51.525])
        self.pDll.Movej_Cmd.argtypes = (ctypes.c_int, ctypes.c_float * 6, ctypes.c_byte,
                                        ctypes.c_float, ctypes.c_bool)
        self.pDll.Movej_Cmd.restype = ctypes.c_int
        ret = self.pDll.Movej_Cmd(self.nSocket, joint, 20, 0, 1)
        logging.info(f'运动至激光笔工具上方：{ret}')

        #   准备安装激光笔
        self.pDll.Set_IO_State.argtypes = (ctypes.c_int, ctypes.c_int, ctypes.c_byte,
                                           ctypes.c_bool, ctypes.c_bool)
        self.pDll.Set_IO_State.restype = ctypes.c_int
        ret = self.pDll.Set_IO_State(self.nSocket, 0, 3, True, 1)
        logging.info(f'准备安装激光笔：{ret}')

        #   Movel安装激光笔
        po1 = POSE()
        po1.px, po1.py, po1.pz, po1.rx, po1.ry, po1.rz = [-0.040369, 0.368299, 0.29172, -3.13, -0.007, 3.112]
        self.pDll.Movel_Cmd.argtypes = (ctypes.c_int, POSE, ctypes.c_byte, ctypes.c_float, ctypes.c_int)
        self.pDll.Movel_Cmd.restype = ctypes.c_int
        ret = self.pDll.Movel_Cmd(self.nSocket, po1, 20, 0, 1)
        logging.info(f'Movel安装激光笔：{ret}')

        #   安装激光笔
        ret = self.pDll.Set_IO_State(self.nSocket, 0, 3, False, 1)
        logging.info(f'安装激光笔：{ret}')

        #   Movel退回
        po2 = POSE()
        po2.px, po2.py, po2.pz, po2.rx, po2.ry, po2.rz = [-0.040369, 0.368299, 0.29172, -3.13, -0.007, 3.112]
        ret = self.pDll.Movel_Cmd(self.nSocket, po2, 20, 0, 1)
        logging.info(f'Movel退回：{ret}')

    def tool_frame(self):
        """
        工具坐标系建立和切换
        :return:
        """
        self.pDll.Manual_Set_Tool_Frame.argtypes = (
            ctypes.c_int, ctypes.POINTER(ctypes.c_char * 5), ctypes.POINTER(POSE),
            ctypes.c_float, ctypes.c_float, ctypes.c_float, ctypes.c_float,
            ctypes.c_int)
        self.pDll.Manual_Set_Tool_Frame.restype = ctypes.c_int

        pose = POSE()
        pose.px, pose.py, pose.pz, pose.rx, pose.ry, pose.rz = [0, 0, 1, 0, 0, 0]
        str_buf = ctypes.create_string_buffer("test".encode('utf-8'))
        ret = self.pDll.Manual_Set_Tool_Frame(self.nSocket, str_buf, pose, 0, 0, 0, 0, 1)
        logging.info(f'设置工具坐标系：{ret}')

        #   切换坐标系
        self.pDll.Change_Tool_Frame.argtypes = (ctypes.c_int, ctypes.POINTER(ctypes.c_char * 5), ctypes.c_int)
        self.pDll.Change_Tool_Frame.restype = ctypes.c_int
        ret = self.pDll.Change_Tool_Frame(self.nSocket, str_buf, 1)
        logging.info(f'切换工具坐标系：{ret}')

        # 获取当前坐标系
        self.pDll.Get_Current_Tool_Frame.argtypes = (ctypes.c_int, ctypes.POINTER(FRAME))
        self.pDll.Get_Current_Tool_Frame.restype = ctypes.c_int
        tool_frame = FRAME()
        self.pDll.Get_Current_Tool_Frame(self.nSocket, tool_frame)
        logging.info(f'当前工具坐标系：{tool_frame.frame_name}')

    def close(self):
        self.pDll.Arm_Socket_Close(self.nSocket)


def main():
    ROT1 = Arm(ip='192.168.1.19')
    ROT1.tool_change()
    ROT1.tool_frame()
    ROT1.close()


if __name__ == "__main__":
    main()
